from sensor_msgs.msg import LaserScan
from floor_set.msg import Floor
from std_msgs.msg import String
from lib.calcPoints import calcPoints
from lib.getCenters import getCenters
from lib.PID import PIDController
from lib.tool import calcRealAngle, calcValueAngle, calcSpeedByAngle
import rospy
import math
import os
import sys
import termios
import tty
import cv2
import json
import time
import atexit

# 常量
TTY_PATH = "/dev/tty"
STTY_US = "stty raw -echo -F"
STTY_DEF = "stty -raw echo -F"
STOP_DELAY = 5
STOP = -1
MID_ANGLE = 80
MAX_ANGLE = 160
validDistance = 400
Pi = 3.1415926
DBSCAN_CONFIG = {
    "eps": 25,
    "min_samples": 5,
    "debug": False,
    "size": (validDistance, validDistance),
}

# PID arguments
kp = 0.76
ki = 0.012
kd = 0.055

pid = PIDController(-kp, -ki, -kd)

isDataReceived: bool = False
steeringAngle: int = 0
# inputChar = 0
currentSpeed: int = 2100
speedRange = 50
isGetInput: bool = False
headlight: bool = False



def CamCtlCallback(msg) -> None:
    global STOP, headlight
    # headlight = not headlight
    msg = json.loads(msg.data)
    if msg['yellow_line']:
        STOP = time.time() + 20040217
    STOP = time.time() + STOP_DELAY

def BtnCtlCallback(msg) -> None:
    #return
    global STOP, headlight
    print("[BUTTON] Button pressed")
    headlight = not headlight
    STOP = time.time() + STOP_DELAY


def LidarCallback(msg: LaserScan) -> None:
    """
    Lidar Calback function

    Args:
        msg: LaserScan for senser.msg
    Returns:
        None
    """
    # start_time = time.time()

    global isDataReceived, steeringAngle, STOP

    if time.time() < STOP:
        return

    isDataReceived = True

    foundLeftObstacle = 0
    foundRightObstacle = 0

    data = list(map(lambda x: x * 100, msg.ranges[:1000]))
    points = calcPoints(data)

    centers, img = getCenters(points, validDistance, **DBSCAN_CONFIG)

    if DBSCAN_CONFIG["debug"]:
        cv2.imshow("DEBUG", img)
        cv2.waitKey(1)

    closestCenters = [validDistance, validDistance]
    closestDistances = [validDistance, validDistance]
    closestAngles = [0, 0]

    for center in centers:
        x = center[0]
        y = center[1]
        distance = math.sqrt(x * x + y * y)
        angle = math.degrees(math.atan2(y, x))
        if distance > validDistance or y < 0:
            continue

        if x < 0:
            if distance < closestDistances[0]:
                closestCenters[0] = center
                closestDistances[0] = distance
                closestAngles[0] = angle - 90
                if closestAngles[0] < 30:
                    continue
                foundLeftObstacle = 1
        elif x > 0:
            if distance < closestDistances[1]:
                closestCenters[1] = center
                closestDistances[1] = distance
                closestAngles[1] = 90 - angle
                if closestAngles[1] < 30:
                    continue
                foundRightObstacle = 1



    if foundLeftObstacle and foundRightObstacle:
        value = abs(closestCenters[1][0]) - abs(closestCenters[0][0])
        steeringAngle = pid.calculate(0, value)
    elif foundLeftObstacle and not foundRightObstacle:
        value = abs(closestCenters[0][0])
        steeringAngle = pid.calculate(0, value)
        print("[Info] Left MAX")
    elif not foundLeftObstacle and foundRightObstacle:
        value = -abs(closestCenters[1][0])
        steeringAngle = pid.calculate(0, value)
        print("[Info] Right MAX")
    else:
        steeringAngle = 0
        print("[Inof] No obstacle")
    
    # print(f"{round(time.time() - start_time,2)} Hz")


def readChar() -> str:
    """
    读取键盘输入的字符

    Args:
        None
    Returns:
        char: 输入的字符
    """
    global isGetInput
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    try:
        tty.setraw(sys.stdin.fileno())
        ch = sys.stdin.read(1)
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
        isGetInput = False
    return ch


rospy.init_node("lidar_node")
pub = rospy.Publisher("car_vel", Floor, queue_size=2)
lidar_sub = rospy.Subscriber("/scan", LaserScan, LidarCallback)
print("[INFO] Scan Start")
visual_sub = rospy.Subscriber("/visual", String, CamCtlCallback)
btn_sub = rospy.Subscriber("/btn", String, BtnCtlCallback)

def cleanup():
    msg = Floor()
    msg.speed = 1500
    msg.angle = MID_ANGLE
    msg.horn = 0
    msg.headlight = 0
    msg.stop = 1
    pub.publish(msg)
    os.system(STTY_DEF + " " + TTY_PATH)

atexit.register(cleanup)

if __name__ == "__main__":
    print("[MAIN]")
    try:
        while not rospy.is_shutdown():
            # inputChar = readChar()
            # if inputChar == "q":
            #     kp += 0.01
            # elif inputChar == "a":
            #     kp -= 0.01
            # elif inputChar == "w":
            #     ki += 0.01
            # elif inputChar == "s":
            #     ki -= 0.01
            # elif inputChar == "e":
            #     kd += 0.01
            # elif inputChar == "d":
            #     kd -= 0.01
            # elif inputChar == "l":
            #     print("BYE")
            #     break

            # if isGetInput:
            #     kp, ki, kd = round(kp, 4), round(ki, 4), round(kd, 4)
            #     pid = PIDController(kp, ki, kd)
            #     print(f"[PID] kp: {kp}\tki: {ki}\tkd: {kd}")
            #     isGetInput = False

            if isDataReceived:
                angle = int(MID_ANGLE - steeringAngle)
                msg = Floor()
                msg.speed = calcSpeedByAngle(angle, currentSpeed, speedRange)
                msg.angle = angle
                msg.headlight = 1 if headlight else 0
                msg.stop = 0
                pub.publish(msg)
                realAngle = calcRealAngle(steeringAngle)
                print(
                    f"[{'R' if realAngle > 0 else 'L'}] Error: {round(pid.last_error,2)} angle: {round(realAngle,2)} Speed:{msg.speed}"
                )
                isDataReceived = False
            
            if time.time() < STOP:
                angle = int(MID_ANGLE - steeringAngle)
                msg = Floor()
                msg.speed = 1500
                msg.angle = angle
                msg.headlight = 1 if headlight else 0
                msg.stop = 1
                pub.publish(msg)
                print(f"[STOP] {STOP - time.time():.2f}s for stop")
            rospy.rostime.wallsleep(0.01)
    finally:
        msg = Floor()
        msg.speed = 1500
        msg.angle = MID_ANGLE
        msg.horn = 0
        msg.headlight = 0
        msg.stop = 1
        pub.publish(msg)
        os.system(STTY_DEF + " " + TTY_PATH)

